package states;
import monitor.StateVariableMonitor;
import gui.OpCom;
import regulators.BallAndBeamRegul;
import regulators.BeamRegul;
import se.lth.control.realtime.DigitalIn;
import se.lth.control.realtime.DigitalOut;
import stateMachine.StateMachine;
public class Throw extends StateClass implements State {

	int counter;
	double ref1 = 0;

	public Throw(DigitalIn initialPosSensor, DigitalOut digitalSolenoid, BeamRegul beam,
			BallAndBeamRegul ballAndBeam, OpCom op, StateVariableMonitor stateVariableMonitor) {
		super(initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor);
	}

	@Override
	public void excecute() {
		ref1-=0.20;
		beam.setRef(ref1);// 2.07
	}

	@Override
	public boolean stateOfExcecution() {
		return beam.controllerStatMonitor.hasReached(3);
	}

	@Override
	public void initialExcecution(){
		op.displayStep("Throw");
		double ref = -3;
		counter = 0;
		ballAndBeam.setRef(ref);

		while( counter < 500 ){
			double ballPos = ballAndBeam.controllerStatMonitor.getBallposition1();
			if( ballPos <= ref +1 && ballPos >= ref -1){
				counter++;
			}
		}
		ballAndBeam.shutDown();
		beam.restart();
	}

	@Override
	public void nextState() {
		beam.setRef(7);
		while(!beam.controllerStatMonitor.hasReached(10)){}
		setCurrentState(new ResetState(initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor));

	}


	@Override
	public void suspend() {
		beam.shutDown();

	}

	@Override
	public void resume() {
		this.initialExcecution();

	}
}